#define SHOW_CLOUD false


#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/kdtree/kdtree_flann.h>
    
int user_data;

std::vector<float> vec;
float * pt;
int n;
float radius_grasp=0.006, radius_centroid=0.004;
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
   

int findClosest(pcl::visualization::PCLVisualizer& viewer, int nx) {
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ>); 
	pcl::io::loadPCDFile("scene.pcd", *cloud2); 
	pcl::KdTree<pcl::PointXYZ>::Ptr tree_ (new pcl::KdTreeFLANN<pcl::PointXYZ>); 
	tree_->setInputCloud(cloud2); 

	std::vector<int> nn_indices (1); 
	std::vector<float> nn_dists (1); 
	
	tree_->nearestKSearch(pcl::PointXYZ(pt[4*nx], pt[4*nx+1], pt[4*nx+2]), 1, nn_indices, nn_dists); 


	//-----
	float rightX = cloud2->at(1).x;
	int i;
	for(i = 1; i < cloud2->size(); i++){

		if(rightX < cloud2->at(i).x)
			rightX = cloud2->at(i).x;

	}
	//-----

	std::stringstream sx; sx<<"clos"<<nx;
	viewer.addSphere (pcl::PointXYZ(rightX, cloud->points[nn_indices[0]].y, cloud->points[nn_indices[0]].z),
		radius_grasp*3, 0, 0, 1.0, sx.str(), 0);
	
	/*viewer.addSphere (pcl::PointXYZ(cloud->points[nn_indices[0]].x, cloud->points[nn_indices[0]].y, cloud->points[nn_indices[0]].z),
		radius_grasp, 0, 0, 1.0, sx.str(), 0);*/
	
	vec.push_back(cloud->points[nn_indices[0]].x);
	vec.push_back(cloud->points[nn_indices[0]].y);
	vec.push_back(cloud->points[nn_indices[0]].z);
	
	sx<<"other"<<nx;
	float x=pt[4*nx]+pt[4*nx+3]*(pt[4*nx]-cloud->points[nn_indices[0]].x);
	float y=pt[4*nx+1]+pt[4*nx+3]*(pt[4*nx+1]-cloud->points[nn_indices[0]].y);
	float z=pt[4*nx+2]+pt[4*nx+3]*(pt[4*nx+2]-cloud->points[nn_indices[0]].z);
	viewer.addSphere (pcl::PointXYZ(x, y, z), radius_grasp, 0, 0, 1.0, sx.str(), 0);
	return 0;

}

void 
viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
{
    viewer.setBackgroundColor (1.0, 1.0, 1.0);
    pcl::PointXYZ o;
	std::stringstream ss1;
	for (int i=0; i<n; i++) {
		o.x = pt[4*i];
		o.y = pt[4*i+1];
		o.z = pt[4*i+2];
		ss1<<"centroid"<<i;
		viewer.addSphere (o, radius_centroid, 1.0, 0, 0, ss1.str(), 0);
		findClosest(viewer,i);

	}
}
    
void 
viewerPsycho (pcl::visualization::PCLVisualizer& viewer)
{
    static unsigned count = 0;
    std::stringstream ss;
    ss << "Once per viewer loop: " << count++;
    viewer.removeShape ("text", 0);
    viewer.addText (ss.str(), 200, 300, "text", 0);
    
    //FIXME: possible race condition here:
    user_data++;
}

    
std::vector<float> displayGrasp (float * point, int number)
{
    pcl::io::loadPCDFile ("scene.pcd", *cloud);
	pt=point;
    n=number;
	int i;

	if(SHOW_CLOUD){
		pcl::visualization::CloudViewer viewer("Grasp Points");
    
		//blocks until the cloud is actually rendered
		viewer.showCloud(cloud);
    
		//use the following functions to get access to the underlying more advanced/powerful
		//PCLVisualizer
    
		//This will only get called once
		viewer.runOnVisualizationThreadOnce (viewerOneOff);
    
		//This will get called once per visualization iteration
		viewer.runOnVisualizationThread (viewerPsycho);
		while (!viewer.wasStopped ())
		{
		//you can also do cool processing here
		//FIXME: Note that this is running in a separate thread from viewerPsycho
		//and you should guard against race conditions yourself...
		
	
		user_data++;
		}
	}
	else{

		for(i = 0; i < n; i++){

			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ>); 
			pcl::io::loadPCDFile("scene.pcd", *cloud2); 
			pcl::KdTree<pcl::PointXYZ>::Ptr tree_ (new pcl::KdTreeFLANN<pcl::PointXYZ>); 
			tree_->setInputCloud(cloud2); 

			std::vector<int> nn_indices (1); 
			std::vector<float> nn_dists (1); 

			tree_->nearestKSearch(pcl::PointXYZ(pt[4*i], pt[4*i+1], pt[4*i+2]), 1, nn_indices, nn_dists); 

			vec.push_back(cloud->points[nn_indices[0]].x);
			vec.push_back(cloud->points[nn_indices[0]].y);
			vec.push_back(cloud->points[nn_indices[0]].z);

		}

	}
    return vec;
}